#include "./PORT/port.h"
#include "../System/config.h"
#include "./BSP/TB6612/tb6612.h"

const uint8_t tb6612_control_pins[][2] = {CFG_MOTOR_CONTROL_PIN};
const uint8_t tb6612_pwm_pin[] = {CFG_MOTOR_PWM_PIN};
const uint8_t tb6612_pwm_tim_id[] = {CFG_MOTOR_PWM_TIM_ID};
const uint8_t tb6612_pwm_tim_ch[] = {CFG_MOTOR_PWM_CHANNEL};

/**
 * @brief       TB6612电机驱动初始化函数
 * @note        该函数会初始化TB6612控制引脚和PWM引脚
 * @retval      无
 */
void tb6612_init(void)
{
    for (uint8_t i = 0; i < CFG_MOTOR_NUM; i++)
    {
        gpio_pin_init(tb6612_control_pins[i][0], GPIO_MODE_OUT, GPIO_OTYPE_PP, GPIO_SPEED_LOW, GPIO_PUPD_PU);
        gpio_pin_init(tb6612_control_pins[i][1], GPIO_MODE_OUT, GPIO_OTYPE_PP, GPIO_SPEED_LOW, GPIO_PUPD_PU);
    }

    for (uint8_t i = 0; i < CFG_MOTOR_NUM; i++)
    {
        tim_pwm_pin_init(tb6612_pwm_tim_id[i], tb6612_pwm_tim_ch[i], tb6612_pwm_pin[i], 100 - 1, 10000 / CFG_MOTOR_FREQUENCY * 84 - 1);
        tim_pwm_set_ccr(tb6612_pwm_tim_id[i], tb6612_pwm_tim_ch[i], 0);
    }

    for (uint8_t i = 0; i < CFG_MOTOR_NUM; i++)
    {
        tb6612_set_direction(i, CFG_MOTOR_DIRECTION_STOP);
    }
}

/**
 * @brief       设置TB6612电机驱动方向
 * @param       motor_id: 电机编号, 从1开始
 * @param       direction: 电机方向, 0:停止 1:正转, 2:反转
 * @retval      无
 */
void tb6612_set_direction(uint8_t motor_id, uint8_t direction)
{
    if (direction == CFG_MOTOR_DIRECTION_FORWARD) // 正转
    {
        gpio_write_pin(tb6612_control_pins[motor_id][0], 1);
        gpio_write_pin(tb6612_control_pins[motor_id][1], 0);
    }
    if (direction == CFG_MOTOR_DIRECTION_BACKWARD) // 反转
    {
        gpio_write_pin(tb6612_control_pins[motor_id][0], 0);
        gpio_write_pin(tb6612_control_pins[motor_id][1], 1);
    }
    if (direction == CFG_MOTOR_DIRECTION_STOP) // 停止
    {
        gpio_write_pin(tb6612_control_pins[motor_id][0], 0);
        gpio_write_pin(tb6612_control_pins[motor_id][1], 0);
    }
}

/**
 * @brief       设置TB6612电机驱动PWM值
 * @param       motor_id: 电机编号, 从1开始
 * @param       value: 占空比 -> 0~100
 * @retval      无
 */
void tb6612_set_pwm_value(uint8_t motor_id, uint16_t value)
{
    tim_pwm_set_ccr(tb6612_pwm_tim_id[motor_id], tb6612_pwm_tim_ch[motor_id], value);
}
